#!/usr/bin/env python3

import rospy                            # 导入ROS Python客户端库，这是所有ROS节点所必需的
from DrEmpower_can.msg import set_angle # 从DrEmpower_can功能包中导入set_angle消息类型
# import DrEmpower as dr                # 注释掉了这里的导入，因为在回调函数中重新导入了

def callback(data):
    """
    ROS话题回调函数，当收到/set_angle话题的消息时被调用
    参数:
        data: 接收到的set_angle类型的消息对象，包含控制电机的所有参数
    """
    import DrEmpower as dr                                                                                  # 在回调函数内部导入DrEmpower库，这样可以确保在ROS节点初始化后才加载驱动
    dr.set_angle(id_num=data.id_num, angle=data.angle, speed=data.speed, param=data.param, mode=data.mode)  # 调用DrEmpower库的set_angle函数，将ROS消息的参数传递给电机驱动函数
    rospy.loginfo("set_angle_node")                                                                         # 记录信息到ROS日志系统，表示收到了设置角度指令

def listener():
    """
    初始化ROS节点并设置消息订阅者
    """
    rospy.init_node("set_agnle", anonymous=True)        # 初始化ROS节点，节点名为"set_agnle"，anonymous=True确保节点名称唯一（会添加随机后缀）
    rospy.Subscriber("set_angle", set_angle, callback)  # 创建订阅者，订阅"set_angle"话题，消息类型为set_angle，收到消息时调用callback函数
    rospy.spin()                                        # 保持节点运行，等待并处理 incoming 消息（进入循环监听状态）


if __name__ == '__main__':                  # Python的标准写法，当此脚本被直接运行时执行以下代码
    listener()                              # 启动监听器